#include "DiffMuscle.h"
#include <OpenSim/Common/SimmMacros.h>
#include <OpenSim/Common/DebugUtilities.h>
#include <OpenSim/Simulation/Model/Model.h>
#include <OpenSim/Common/Storage.h>
#include <iostream>

using namespace std;
using namespace OpenSim;


//=============================================================================

// PLUGIN - SPECIFIC FUNCTIONS

//=============================================================================

//_____________________________________________________________________________

/**

 * Perform some set up functions that happen after the

 * object has been deserialized or copied.

 * @param aModel model containing this DiffMuscle.

 */


const int DiffMuscle::STATE_ACTIVATION_FAST_FIBER = 2;
const int DiffMuscle::STATE_ACTIVATION_SLOW_FIBER = 3;

void DiffMuscle::setup(Model& aModel)

{
       // Base class

       ActivationFiberLengthMuscle::setup(aModel);
}



void DiffMuscle::createSystem(SimTK::MultibodySystem& system) const

{
       ActivationFiberLengthMuscle::createSystem(system);

       DiffMuscle* mutableThis = const_cast<DiffMuscle *>(this);

       // Cache the computed passive muscle force
       // note the total muscle force is the tendon force and is already a cached variable of the actuator

       mutableThis->addCacheVariable<double>("passiveForce", 0.0, SimTK::Stage::Velocity);

}





int DiffMuscle::getStateVariableYIndex(int index) const

{
       if (index==0)

             return _model->getMultibodySystem().getDefaultState().getZStart()+_zIndex;

       if (index ==1)

             return _model->getMultibodySystem().getDefaultState().getZStart()+_zIndex+1;

       throw Exception("Trying to get Coordinate State variable YIndex for Coorindate "+getName()+" at undefined index"); 
}



/* David Modification

 * Compute the derivatives of the muscle states.

 * @param s  system state 

 * @param index 

 */

SimTK::Vector DiffMuscle::computeStateVariableDerivatives(const SimTK::State &s) const

{
       SimTK::Vector derivs(getNumStateVariables());

       derivs[0] = getActivationDeriv(s);

       derivs[1] = getFiberLengthDeriv(s);

       return derivs;
}


//_____________________________________________________________________________

/**
David Modification
 * Constructor.

 */

DiffMuscle::DiffMuscle():
	ActivationFiberLengthMuscle(),

	_fmaxTendonStrain( _fmaxTendonStrainProp.getValueDbl() ),

	_fmaxMuscleStrain( _fmaxMuscleStrainProp.getValueDbl() ),

	_damping( _dampingProp.getValueDbl() ),

	_flen( _flenProp.getValueDbl() ),
	
	_kShapePassive( _kShapePassiveProp.getValueDbl() ),
	
	_activationTimeConstantFastFiber( _activationTimeConstantProp[0].getValueDbl() ),
	_activationTimeConstantSlowFiber( _activationTimeConstantProp[1].getValueDbl() ),

	_deactivationTimeConstantFastFiber( _deactivationTimeConstantProp[0].getValueDbl() ),
	_deactivationTimeConstantSlowFiber( _deactivationTimeConstantProp[1].getValueDbl() ),

	_vmaxFastFiber( _vmaxProp[0].getValueDbl() ),
	_vmaxSlowFiber( _vmaxProp[1].getValueDbl() ),

	_vmax0FastFiber( _vmax0Prop[0].getValueDbl() ),
	_vmax0SlowFiber( _vmax0Prop[0].getValueDbl() ),

	_kShapeActiveFastFiber( _kShapeActiveProp[0].getValueDbl() ),
	_kShapeActiveSlowFiber( _kShapeActiveProp[1].getValueDbl() ),

	_afFastFiber( _afProp[0].getValueDbl() ),
	_afSlowFiber( _afProp[1].getValueDbl() )
	
	{
	setNull();

	setupProperties();
	}



DiffMuscle::DiffMuscle(const std::string &aName,double aMaxIsometricForce,double aOptimalFiberLength,double aTendonSlackLength,double aPennationAngle) :

		
	ActivationFiberLengthMuscle(),

	_fmaxTendonStrain( _fmaxTendonStrainProp.getValueDbl() ),

	_fmaxMuscleStrain( _fmaxMuscleStrainProp.getValueDbl() ),

	_damping( _dampingProp.getValueDbl() ),

	_flen( _flenProp.getValueDbl() ),
	
	_kShapePassive( _kShapePassiveProp.getValueDbl() ),
	
	_activationTimeConstantFastFiber( _activationTimeConstantProp[0].getValueDbl() ),
	_activationTimeConstantSlowFiber( _activationTimeConstantProp[1].getValueDbl() ),

	_deactivationTimeConstantFastFiber( _deactivationTimeConstantProp[0].getValueDbl() ),
	_deactivationTimeConstantSlowFiber( _deactivationTimeConstantProp[1].getValueDbl() ),

	_vmaxFastFiber( _vmaxProp[0].getValueDbl() ),
	_vmaxSlowFiber( _vmaxProp[1].getValueDbl() ),

	_vmax0FastFiber( _vmax0Prop[0].getValueDbl() ),
	_vmax0SlowFiber( _vmax0Prop[0].getValueDbl() ),

	_kShapeActiveFastFiber( _kShapeActiveProp[0].getValueDbl() ),
	_kShapeActiveSlowFiber( _kShapeActiveProp[1].getValueDbl() ),

	_afFastFiber( _afProp[0].getValueDbl() ),
	_afSlowFiber( _afProp[1].getValueDbl() )
	
	{
	setNull();

	setupProperties();

	setName(aName);

	setMaxIsometricForce(aMaxIsometricForce);

	setOptimalFiberLength(aOptimalFiberLength);

	setTendonSlackLength(aTendonSlackLength);

	setPennationAngleAtOptimalFiberLength(aPennationAngle);
}



//_____________________________________________________________________________

/**

 * Destructor.

 */

DiffMuscle::~DiffMuscle()

{

}



//_____________________________________________________________________________

/**

 * Copy constructor.

 *

 * @param aMuscle DiffMuscle to be copied.

 */

DiffMuscle::DiffMuscle(const DiffMuscle &aMuscle) :
	ActivationFiberLengthMuscle(aMuscle),
	
	_fmaxTendonStrain( _fmaxTendonStrainProp.getValueDbl() ),
	_fmaxMuscleStrain( _fmaxMuscleStrainProp.getValueDbl() ),

	_damping( _dampingProp.getValueDbl() ),

	_flen( _flenProp.getValueDbl() ),
	
	_kShapePassive( _kShapePassiveProp.getValueDbl() ),
	
	_activationTimeConstantFastFiber( _activationTimeConstantProp[0].getValueDbl() ),
	_activationTimeConstantSlowFiber( _activationTimeConstantProp[1].getValueDbl() ),

	_deactivationTimeConstantFastFiber( _deactivationTimeConstantProp[0].getValueDbl() ),
	_deactivationTimeConstantSlowFiber( _deactivationTimeConstantProp[1].getValueDbl() ),

	_vmaxFastFiber( _vmaxProp[0].getValueDbl() ),
	_vmaxSlowFiber( _vmaxProp[1].getValueDbl() ),

	_vmax0FastFiber( _vmax0Prop[0].getValueDbl() ),
	_vmax0SlowFiber( _vmax0Prop[0].getValueDbl() ),

	_kShapeActiveFastFiber( _kShapeActiveProp[0].getValueDbl() ),
	_kShapeActiveSlowFiber( _kShapeActiveProp[1].getValueDbl() ),

	_afFastFiber( _afProp[0].getValueDbl() ),
	_afSlowFiber( _afProp[1].getValueDbl() )

{
	
	setNull();

	setupProperties();

	copyData(aMuscle);
}



//_____________________________________________________________________________

/**

 * Copy this muscle point and return a pointer to the copy.

 * The copy constructor for this class is used.

 *

 * @return Pointer to a copy of this DiffMuscle.

 */

Object* DiffMuscle::copy() const

{

	DiffMuscle *musc = new DiffMuscle(*this);

	return(musc);

}



//=============================================================================

// CONSTRUCTION METHODS

//=============================================================================

//_____________________________________________________________________________

/**

 * Copy data members from one DiffMuscle to another.

 *

 * @param aMuscle DiffMuscle to be copied.

 */

void DiffMuscle::copyData(const DiffMuscle &aMuscle)

{
	_activationTimeConstantFastFiber = aMuscle._activationTimeConstantFastFiber;
	_activationTimeConstantSlowFiber = aMuscle._activationTimeConstantSlowFiber;

	_deactivationTimeConstantFastFiber = aMuscle._deactivationTimeConstantFastFiber;
	_deactivationTimeConstantSlowFiber = aMuscle._deactivationTimeConstantSlowFiber;

	_vmaxFastFiber = aMuscle._vmaxFastFiber;
	_vmaxSlowFiber = aMuscle._vmaxSlowFiber;

	_vmax0FastFiber = aMuscle._vmax0FastFiber;
	_vmax0SlowFiber = aMuscle._vmax0SlowFiber;

	_fmaxTendonStrain = aMuscle._fmaxTendonStrain;

	_fmaxMuscleStrain = aMuscle._fmaxMuscleStrain;

	_kShapeActiveFastFiber = aMuscle._kShapeActiveFastFiber;
	_kShapeActiveSlowFiber = aMuscle._kShapeActiveSlowFiber;

	_kShapePassive = aMuscle._kShapePassive;

	_damping = aMuscle._damping;

	_afFastFiber = aMuscle._afFastFiber;
	_afSlowFiber = aMuscle._afSlowFiber;

	_flen = aMuscle._flen;
}



/**

 * Set the data members of this DiffMuscle to their null values.

 */

void DiffMuscle::setNull()

{

	setType("DiffMuscle");
	setNumStateVariables(4);
	_stateVariableSuffixes[STATE_ACTIVATION_FAST_FIBER] = "activation_fast_fiber";
	_stateVariableSuffixes[STATE_ACTIVATION_SLOW_FIBER] = "activation_slow_fiber";
}

//_____________________________________________________________________________

/**

 * Connect properties to local pointers.

 */

void DiffMuscle::setupProperties()

{
	//array index 0 is for fast fiber; 1 for slow fiber 
	_activationTimeConstantProp[0].setName("ActivationTimeConstantFastFiber");
	_activationTimeConstantProp[0].setValue(0.01);
	_activationTimeConstantProp[0].setComment("time constant for ramping up of muscle activation of fast fiber");
	_propertySet.append(&(_activationTimeConstantProp[0]), "Parameters");

	_activationTimeConstantProp[1].setName("ActivationTimeConstantSlowFiber");
	_activationTimeConstantProp[1].setValue(0.01);
	_activationTimeConstantProp[1].setComment("time constant for ramping up of muscle activation of slow fiber");
	_propertySet.append(&(_activationTimeConstantProp[1]), "Parameters");

	_deactivationTimeConstantProp[0].setName("DeactivationTimeConstantFastFiber");
	_deactivationTimeConstantProp[0].setValue(0.04);
	_deactivationTimeConstantProp[0].setComment("time constant for ramping down of muscle activation of fast fiber");
	_propertySet.append(&(_deactivationTimeConstantProp[0]), "Parameters");

	_deactivationTimeConstantProp[1].setName("DeactivationTimeConstantSlowFiber");
	_deactivationTimeConstantProp[1].setValue(0.04);
	_deactivationTimeConstantProp[1].setComment("time constant for ramping down of muscle activation of slow fiber");
	_propertySet.append(&(_deactivationTimeConstantProp[1]), "Parameters");

	_vmaxProp[0].setName("VmaxFastFiber");
	_vmaxProp[0].setValue(10.0);
	_vmaxProp[0].setComment("maximum contraction velocity of fast fiber at full activation in fiber lengths per second");
	_propertySet.append(&(_vmaxProp[0]), "Parameters");

	_vmaxProp[1].setName("VmaxSlowFiber");
	_vmaxProp[1].setValue(10.0);
	_vmaxProp[1].setComment("maximum contraction velocity of slow fiber at full activation in fiber lengths per second");
	_propertySet.append(&(_vmaxProp[1]), "Parameters");

	_vmax0Prop[0].setName("Vmax0FastFiber");
	_vmax0Prop[0].setValue(5.0);
	_vmax0Prop[0].setComment("maximum contraction velocity of fast fiber at low activation in fiber lengths per second");
	_propertySet.append(&(_vmax0Prop[0]), "Parameters");

	_vmax0Prop[1].setName("Vmax0SlowFiber");
	_vmax0Prop[1].setValue(5.0);
	_vmax0Prop[1].setComment("maximum contraction velocity of slow fiber at low activation in fiber lengths per second");
	_propertySet.append(&(_vmax0Prop[1]), "Parameters");

	_fmaxTendonStrainProp.setName("FmaxTendonStrain");
	_fmaxTendonStrainProp.setValue(0.033);
	_fmaxTendonStrainProp.setComment("tendon strain due to maximum isometric muscle force");
	_propertySet.append(&_fmaxTendonStrainProp, "Parameters");

	_fmaxMuscleStrainProp.setName("FmaxMuscleStrain");
	_fmaxMuscleStrainProp.setValue(0.6);
	_fmaxMuscleStrainProp.setComment("passive muscle strain due to maximum isometric muscle force");
	_propertySet.append(&_fmaxMuscleStrainProp, "Parameters");

	_kShapeActiveProp[0].setName("KshapeActiveFastFiber");
	_kShapeActiveProp[0].setValue(0.5);
	_kShapeActiveProp[0].setComment("shape factor for Gaussian active muscle force-length relationship of fast fiber");
	_propertySet.append(&(_kShapeActiveProp[0]), "Parameters");

	_kShapeActiveProp[1].setName("KshapeActiveSlowFiber");
	_kShapeActiveProp[1].setValue(0.5);
	_kShapeActiveProp[1].setComment("shape factor for Gaussian active muscle force-length relationship of slow fiber");
	_propertySet.append(&(_kShapeActiveProp[1]), "Parameters");

	_kShapePassiveProp.setName("KshapePassive");
	_kShapePassiveProp.setValue(4.0);
	_kShapePassiveProp.setComment("exponential shape factor for passive force-length relationship");
	_propertySet.append(&_kShapePassiveProp, "Parameters");

	_dampingProp.setName("damping");
	_dampingProp.setValue(0.05);
	_dampingProp.setComment("passive damping in the force-velocity relationship");
	_propertySet.append(&_dampingProp, "Parameters");

	_afProp[0].setName("AfFastFiber");
	_afProp[0].setValue(0.3);
	_afProp[0].setComment("force-velocity shape factor of fast fiber");
	_propertySet.append(&(_afProp[0]), "Parameters");

	_afProp[1].setName("AfSlowFiber");
	_afProp[1].setValue(0.3);
	_afProp[1].setComment("force-velocity shape factor of slow fiber");
	_propertySet.append(&(_afProp[1]), "Parameters");


	_flenProp.setName("Flen");
	_flenProp.setValue(1.8);
	_flenProp.setComment("maximum normalized lengthening force");
	_propertySet.append(&_flenProp, "Parameters");

}



//=============================================================================

// OPERATORS

//=============================================================================

//_____________________________________________________________________________

/**

 * Assignment operator.

  * @return Reference to this object.

 */

DiffMuscle& DiffMuscle::operator=(const DiffMuscle &aMuscle)

{

	// BASE CLASS

	ActivationFiberLengthMuscle::operator=(aMuscle);
	copyData(aMuscle);

	return(*this);

}


//=============================================================================

// GET

//=============================================================================



//-----------------------------------------------------------------------------

// ACTIVATION TIME

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the time constant for ramping up of muscle force.

 *

 * @param aActivationTimeConstant The time constant for ramping up of muscle force.

 * @return Whether the time constant was successfully changed.

 */

bool DiffMuscle::setActivationTimeConstant(double aActivationTimeConstant[])

{

	_activationTimeConstantFastFiber = aActivationTimeConstant[0];
	_activationTimeConstantSlowFiber = aActivationTimeConstant[1];
	
	return true;
}



//-----------------------------------------------------------------------------

// DEACTIVATION TIME

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the time constant for ramping down of muscle force.

 *

 * @param aDeactivationTimeConstant The time constant for ramping down of muscle force.

 * @return Whether the time constant was successfully changed.

 */

bool DiffMuscle::setDeactivationTimeConstant(double aDeactivationTimeConstant[])

{

	_deactivationTimeConstantFastFiber = aDeactivationTimeConstant[0];
	_deactivationTimeConstantSlowFiber = aDeactivationTimeConstant[1];


	return true;

}





//-----------------------------------------------------------------------------

// MAX CONTRACTION VELOCITY

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the maximum contraction velocity of the fibers, in optimal fiber lengths per second.

 *

 * @param aVmax The maximum contraction velocity of the fibers, in optimal fiber lengths per second.

 * @return Whether the maximum contraction velocity was successfully changed.

 */

bool DiffMuscle::setVmax(double aVmax[])

{

	_vmaxFastFiber = aVmax[0];
	_vmaxSlowFiber = aVmax[1];
	
	return true;

}



//-----------------------------------------------------------------------------

// MAX CONTRACTION VELOCITY AT LOW ACTIVATION

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the maximum contraction velocity at low activation of the fibers, in optimal fiber lengths per second.

 *

 * @param aVmax The maximum contraction velocity at low activation of the fibers, in optimal fiber lengths per second.

 * @return Whether the maximum contraction velocity was successfully changed.

 */

bool DiffMuscle::setVmax0(double aVmax0[])

{

	_vmax0FastFiber = aVmax0[0];
	_vmax0SlowFiber = aVmax0[1];

	return true;

}



//-----------------------------------------------------------------------------

// TENDON STRAIN

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the tendon strain due to maximum isometric muscle force.

 *

 * @param aFmaxTendonStrain The tendon strain due to maximum isometric muscle force.

 * @return Whether the tendon strain was successfully changed.

 */

bool DiffMuscle::setFmaxTendonStrain(double aFmaxTendonStrain)

{

	_fmaxTendonStrain = aFmaxTendonStrain;

	return true;

}



//-----------------------------------------------------------------------------

// MUSCLE STRAIN

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the passive muscle strain due to maximum isometric muscle force.

 *

 * @param aFmaxMuscleStrain The passive muscle strain due to maximum isometric muscle force.

 * @return Whether the passive muscle strain was successfully changed.

 */

bool DiffMuscle::setFmaxMuscleStrain(double aFmaxMuscleStrain)

{

	_fmaxMuscleStrain = aFmaxMuscleStrain;

	return true;

}



//-----------------------------------------------------------------------------

// SHAPE FACTOR ACTIVE

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the shape factor for Gaussian active muscle force-length relationship.

 *

 * @param aKShapeActive The shape factor for Gaussian active muscle force-length relationship.

 * @return Whether the shape factor was successfully changed.

 */

bool DiffMuscle::setKshapeActive(double aKShapeActive[])

{

	_kShapeActiveFastFiber = aKShapeActive[0];
	_kShapeActiveSlowFiber = aKShapeActive[1];

	return true;

}



//-----------------------------------------------------------------------------

// SHAPE FACTOR PASSIVE

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the shape factor for Gaussian passive muscle force-length relationship.

 *

 * @param aKshapePassive The shape factor for Gaussian passive muscle force-length relationship.

 * @return Whether the shape factor was successfully changed.

 */

bool DiffMuscle::setKshapePassive(double aKshapePassive)

{

	_kShapePassive = aKshapePassive;

	return true;

}



//-----------------------------------------------------------------------------

// DAMPING

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the damping factor related to maximum contraction velocity.

 *

 * @param aDamping The damping factor related to maximum contraction velocity.

 * @return Whether the damping factor was successfully changed.

 */

bool DiffMuscle::setDamping(double aDamping)

{

	_damping = aDamping;
	
	return true;

}



//-----------------------------------------------------------------------------

// FORCE-VELOCITY SHAPE FACTOR

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the force-velocity shape factor.

 *

 * @param aAf The force-velocity shape factor.

 * @return Whether the shape factor was successfully changed.

 */

bool DiffMuscle::setAf(double aAf[])

{

	_afFastFiber = aAf[0];
	_afSlowFiber = aAf[1];

	return true;

}



//-----------------------------------------------------------------------------

// FORCE-VELOCITY SHAPE FACTOR

//-----------------------------------------------------------------------------

//_____________________________________________________________________________

/**

 * Set the maximum normalized lengthening force.

 *

 * @param aFlen The maximum normalized lengthening force.

 * @return Whether the maximum normalized lengthening force was successfully changed.

 */

bool DiffMuscle::setFlen(double aFlen)

{

	_flen = aFlen;

	return true;

}

double  DiffMuscle::computeActuation(const SimTK::State& s) const

{

	double tendonForce;

	double passiveForce;

	double activationDeriv;

	double fiberLengthDeriv;



	double norm_tendon_length, ca;

	double norm_muscle_tendon_length, pennation_angle;

	double excitation = getExcitation(s);



	/* Normalize the muscle states */

	double activation = getActivation(s);

	double normFiberLength = getFiberLength(s) / _optimalFiberLength;


	// Maximum contraction velocity is an activation scaled value?

	double VmaxFastFiber = _vmaxFastFiber;
	double VmaxSlowFiber = _vmaxSlowFiber;

	//MM excitation bounds check
	
	if (excitation < 0.0){

		excitation = 0.0;

	}

	if(excitation > 1.0){

		excitation = 1.0;

	}

	double activation_min = 0.01; 

	if (activation >= 1.0){	

		activation = 1.0;	//This will not affect the state, unfortunately, but it will prevent

	}						//the model from using an activation greater than 1

	

	if (activation <= activation_min){

		activation = activation_min; 	 

	}								
	
	//This will prevent a lot of singularites in later code
	/* Compute normalized muscle state derivatives */

	double tau_a = 0.0; //MM updated according to paper

	if (excitation >= activation) {

		tau_a = _activationTimeConstantFastFiber * (0.5 + 1.5*activation);	//MM updated according to paper

		activationDeriv = (excitation - activation) / tau_a;		//MM updated according to paper

	} else {

		tau_a = _deactivationTimeConstantFastFiber/(0.5+1.5*activation);		//MM updated according to paper

		activationDeriv = (excitation - activation) / tau_a;		//MM updated according to paper
	}



	//MM: bounds check on activation

	//MM FIX: hard coded to help Edith quickly

	//MM: I'd also set activation directly, but I cannot as it is a state variable

	if (activation >= 1.0 && activationDeriv > 0.0){	

		activationDeriv = 0.0;

	}

	if (activation <= activation_min && activationDeriv < 0.0){

		activationDeriv = 0.0;

		activation = activation_min; 	//This will not affect the state, but it will

						//prevent a lot of singularites in later code

	}	

	//MM update calcPennation: it returns 0 when normFiberLength = 0, should return Pi/2

	if(normFiberLength > 0.0){

		pennation_angle = calcPennation( normFiberLength, 1.0, _pennationAngleAtOptimal);

	}else{

		pennation_angle = SimTK_PI/2;

	}

	ca = cos(pennation_angle);	



	//MM bounding ca away from singularity

	//MM FIX: hard coded constant of cos(89 degrees) quick and dirty for Edith

	double ca_min = 0.0175; //equivalent to 89 degrees.

	if(ca <= ca_min){

		ca = ca_min;

		pennation_angle = acos(ca);

	}

	norm_muscle_tendon_length = getLength(s) / _optimalFiberLength;

	norm_tendon_length = norm_muscle_tendon_length - normFiberLength * ca;



	double tmp_norm_resting_length = _tendonSlackLength / _optimalFiberLength; //MM

	double tmp_tendon_strain =  (norm_tendon_length - tmp_norm_resting_length) / tmp_norm_resting_length; //MM


	tendonForce = calcTendonForce(s,norm_tendon_length);

	passiveForce = calcPassiveForce(s,normFiberLength);

	double activeForce = calcActiveForceFastFiber(s,normFiberLength);


	//MM for generating the dimensionless plots

	double velocity_dependent_force = tendonForce / ca - passiveForce; //MM

	
	fiberLengthDeriv = calcFiberVelocityFastFiber(s,activation,activeForce,velocity_dependent_force);

	
	/* Un-normalize the muscle state derivatives and forces. */

	setActivationDeriv(s, activationDeriv );

	setFiberLengthDeriv(s, VmaxFastFiber*_optimalFiberLength*fiberLengthDeriv ); 



	tendonForce = tendonForce *  _maxIsometricForce;

	setForce(s, tendonForce);

	setTendonForce(s, tendonForce);

	setPassiveForce( s, passiveForce * _maxIsometricForce);


	return( tendonForce );

}



//_____________________________________________________________________________

/**

 * Edited by M.Millard 2011/10.

 * Original version did not match the equations in the paper because it had a

 * very shallow ramp added to it. Now this function matches the Thelen2003 paper

 *

 * From cmg_dt.c - calc_tendon_force_dt

 *

 * CALC_TENDON_FORCE_DT: this routine calculates the force in tendon by finding

 * tendon strain and using it in an exponential function (JBME 2003 - Thelen)

 * FmaxTendonStrain - Function is parameterized by the tendon strain due to maximum isometric muscle force

 * This should be specified as a dynamic parameter in the muscle file

 *

 * @param aNormTendonLength Normalized length of the tendon.

 * @return The force in the tendon.

 */

double DiffMuscle::calcTendonForce(const SimTK::State& s, double aNormTendonLength) const

{



	//MM - This business of normalizing the tendon length with respect to the fiber length is weird

	//It works, but is a candidate for revision.

	double norm_resting_length = _tendonSlackLength / _optimalFiberLength;

	double tendon_strain =  (aNormTendonLength - norm_resting_length) / norm_resting_length;



	

	double kToe = 3.0;

	double eToe = 0.609*_fmaxTendonStrain;

	double Ftoe = 0.333333;

	double klin = 1.712/_fmaxTendonStrain;



	//Compute tendon force

	double tendon_force;

	if (tendon_strain>eToe)

		tendon_force = klin*(tendon_strain-eToe)+Ftoe;

	else if (tendon_strain>0.0) 

		tendon_force = (Ftoe/(exp(kToe)-1.0)) * (exp(kToe*tendon_strain/eToe)-1.0);

	else

		tendon_force=0.;



	//Compute the energy stored in the tendon. 

	//Integrals computed symbolically in muscle_kepew_20111021.mw just to check

	
	double lenTdn	= (tendon_strain+1)*_tendonSlackLength;

	double lenToe	= (eToe+1.0)*_tendonSlackLength;	

	double lenR		= _tendonSlackLength;

	double fiso		=_maxIsometricForce;



	if(s.getTime() >= 0.1){

		double debuggingTime = 1.0; //MM temporary, remove later.

	}



	if (tendon_strain>eToe){

		//compute the energy stored in the toe portion of the tendon strain curve

		double len = lenToe;

		double toePE_len = (fiso*Ftoe/(exp(kToe)-1.0)) * ((lenR*eToe/kToe) * exp(kToe*(len-lenR)/(lenR*eToe)) - len);

		len =  lenR;

		double toePE_0	=  (fiso*Ftoe/(exp(kToe)-1.0)) * ((lenR*eToe/kToe) * exp(kToe*(len-lenR)/(lenR*eToe)) - len);

		double toePEtest = toePE_len-toePE_0;





		//compute the energy stored in the linear section of the tendon strain curve from ..... 0 to len

		len = lenTdn;

		double linPE_len = (1.0/2.0)*(fiso*klin*(len*len)/lenR) + fiso*len*(klin*(-1.0-eToe)+Ftoe);

		//ditto from 0 .... eToe

		len = lenToe;

		double linPE_eToe= (1.0/2.0)*(fiso*klin*(len*len)/lenR) + fiso*len*(klin*(-1.0-eToe)+Ftoe);

		double linPEtest = linPE_len-linPE_eToe;

		

		//compute the total potential energy stored in the tendon

		
	}else if (tendon_strain>0.0){ 

		//PE from 0 .... len

		double len = lenTdn;

		double toePE_len = (_maxIsometricForce*Ftoe/(exp(kToe)-1.0)) * ((lenR*eToe/kToe) * exp(kToe*(len-lenR)/(lenR*eToe)) - len);

		//PE from 0 .... eToe

		len = lenR;

		double toePE_0	=  (_maxIsometricForce*Ftoe/(exp(kToe)-1.0)) * ((lenR*eToe/kToe) * exp(kToe*(len-lenR)/(lenR*eToe)) - len);



		//Compute the total PE stored in the tendon



	}else{

	
	}

	return tendon_force;

}


//_____________________________________________________________________________

/**

 * M.Millard 2011/10.

 * The original version (in Thelen2003Muscle) did not match the equations in the 

 * paper because it had a slightly different equations. Now this function matches 

 * that returned non-zero values even when the normalized muscle length was < 1.

 * The implementation below matches Eqn. 3 of Thelen 2003, and does not develop

 * force until the normalized muscle length is greater than 1.0.

 * 

 * @param aNormFiberLength Normalized length of the muscle fiber.

 * @return The passive force in the muscle fibers.

 */

double DiffMuscle::calcPassiveForce(const SimTK::State& s, double aNormFiberLength) const

{

	double passive_force;



	//Compute the passive force developed by the muscle

	if(aNormFiberLength > 1.0){

		passive_force = (exp(_kShapePassive*(aNormFiberLength-1.0)/_fmaxMuscleStrain)-1.0) / (exp(_kShapePassive)-1.0);

	}else{

		passive_force = 0.0;

	}


	//Compute the potential energy stored in the muscle

	if(aNormFiberLength > 1.0){

		//Shorter variable names to make the equations readable.

		double len = aNormFiberLength*_optimalFiberLength;

		double lenR = _optimalFiberLength;

		double kpe = _kShapePassive;

		double e0 = _fmaxMuscleStrain;

		double fiso = _maxIsometricForce;



		//PE storage at current stretch

		double fpePE_len = (fiso/(exp(kpe)-1))*( (lenR*e0/kpe)*exp( (kpe/e0)*( (len/lenR)-1)) - len); 

		

		//PE when muscle goes slack.

		len = _optimalFiberLength;

		double fpePE_0 = (fiso/(exp(kpe)-1))*( (lenR*e0/kpe)*exp( (kpe/e0)*( (len/lenR)-1)) - len); 

	}else{


	}

	return passive_force;

}



//_____________________________________________________________________________

/**

 * Edited by M.Millard 2011/10.

 * Now includes the option to use spline fit interpolations of active force length

 * 

 * From gmc.dt.c - calc_active_force_dt

 *

 * CALC_ACTIVE_FORCE_DT: this routine calculates the active component of force

 * in the muscle fibers. It uses the current fiber length to interpolate the

 * active force-length curve - described by Gaussian curve as in Thelen, JBME 2003

 * *

 * @param aNormFiberLength Normalized length of the muscle fiber.

 * @return The active force in the muscle fibers.

 */

double DiffMuscle::calcActiveForceFastFiber(const SimTK::State& s, double aNormFiberLength) const

{

	double fal = 0.0;

	double fal_min = 0.01;

	double x=(aNormFiberLength-1.)*(aNormFiberLength-1.);

	fal = exp(-x*x/_kShapeActiveFastFiber);

	//Lower bound on fal to prevent a singularity

	if(fal < fal_min){

		fal = fal_min;

	}



	return fal;

}



//_____________________________________________________________________________

/**

 * M.Millard 2011/10.

 *

 * Presently not included in the muscle model, but would be a useful addition.

 *

 * This function was added to prevent this muscle model from computing contractile

 * element lengths that are shorter than what is physiologically possible 

 * (0.5*optimal_force_length). Without this addition the Thelen model will converge

 * to contractile element lengths that have no lower bound if geometry allows. This

 * function will generate a compressive force using a function that mirrors the

 * muscle's parallel element (but in compression) when the fiber length is less than

 * half of its optimal value.

 * 

 * @param aNormFiberLength Normalized length of the muscle fiber.

 * @return The active force in the muscle fibers.

 *

double DiffMuscle::calcPassiveCompressiveForce(const SimTK::State& s, double aNormFiberLength) const

{

	double minFibLen = 0.5;

	double passive_force = 0.0;

	

	if(aNormFiberLength < minFibLen){

		double x = minFibLen-aNormFiberLength;

		passive_force = (exp(_kShapePassive*(x-1)/_fmaxMuscleStrain)-1) / (exp(_kShapePassive)-1);

	}



	return passive_force;

}

*/

//_____________________________________________________________________________

/**

 * From gmc_dt.c - calc_norm_fiber_velocity_dt

 *

 * CALC_NORM_FIBER_VELOCITY_DT: written by Matthew Millard on 2011/10/13

 * The original function written by (?) had some implementation errors in it.

 * This function has been completely re-written to reflect the force-velocity

 * curve that is actually documented in the Thelen 2003 paper. The only difference

 * is that this model includes a linear extrapolation that the Thelen model does

 * not. The model switches from the Thelen model to a continuous and smooth linear

 * extrapolation at the following locations on the force-velocity curve

 *

 *		-infinity  < fv < 0					: Linear extrapolation

 *				 0 < fv < 0.9*(a*fal*fmax)	: Thelen force velocity curve 

 * 0.9*(a*fal*fmax)< fv						: Linear extrapolation that is a continuous

 *											  and smooth extrapolation of the

 *											  Thelen curves

 *

 * Why make the transition at 0.9? The slopeis not too shallow in this region

 * so transitioning at 0.9 will not cause the integrator undue distress. Why 

 * is the integrator our top priority in this region? Your simulation should not

 * be spending much time beyond this region, as the equilibrium model will never

 * be able to give good results in this region without resulting in 

 * tremendously stiff equations.

 *

 * Dynamic Parameters:

 *   _af - velocity shape factor from Hill's equation

 *   _flen	- Maximum normalized force when muscle is lengthening

 *

 * @param aActivation Activation of the muscle.

 * @param aActiveForce Active force in the muscle fibers.

 * @param aVelocityDependentForce Force value that depends on fiber velocity.

 * @return The velocity of the muscle fibers.

 */

double DiffMuscle::calcFiberVelocityFastFiber(const SimTK::State& s, double aActivation, double aActiveForce, double aVelocityDependentForce) const

{

	//The variable names have all been switched to closely match with the notation in

	//Thelen 2003.

	double a = aActivation;

	double afl = aActivation*aActiveForce;  //afl = a*fl

	double Fm = aVelocityDependentForce;	//Fm = a*fl*fv	

	double Fmlen = _flen;



	double dlce = 0.0;	  //contractile element velocity

	double dlcedFm = 0.0; //partial deriviative of contactile element velocity w.r.t. Fm



	double b = 0;

    double db= 0;



    double Fm_asyC = 0;									//Concentric contraction asymptote

    double Fm_asyE = aActivation*aActiveForce*_flen;	//Eccentric contraction asymptote

	double asyE_thresh = 0.9;	//Should this be user settable?

								//What fraction of Fm_asyE Fv is allowed to have before a linear

								//model is used



		//If fv is in the appropriate region, use Thelen 2003 Eqns 6 & 7 to compute dlce

        if (Fm > Fm_asyC && Fm < Fm_asyE*asyE_thresh){



            if( Fm <= afl ){		//Muscle is concentrically contracting

               b = afl + Fm/_afFastFiber;

               db= 1/_afFastFiber;

			}else{					//Muscle is eccentrically contracting

               b = ((2+2/_afFastFiber)*(afl*Fmlen-Fm))/(Fmlen-1); 

               db= ((2+2/_afFastFiber)*(-1))/(Fmlen-1); 

			}



            dlce = (0.25 + 0.75*a)*(Fm-afl)/b; //Scaling by VMAX is left out, and is post multiplied outside of the function



			//This variable may have future use outside this function

            dlcedFm = (0.25 + 0.75*a)*(1)/b - ((0.25 + 0.75*a)*(Fm-afl)/(b*b))*db;			



		}else{  //Linear extrapolation

				double Fm0 = 0.0; //Last Fm value from the Thelen curve



				//Compute d and db/dFm from Eqn 7. of Thelen2003

				//for the last

				if(Fm <= Fm_asyC){ //Concentrically contracting

					Fm0 = Fm_asyC;

					b = afl + Fm0/_afFastFiber;

					db= 1/_afFastFiber;               

				}else{			 //Eccentrically contracting

					Fm0 = asyE_thresh*Fm_asyE;

					b = ((2+2/_afFastFiber)*(afl*Fmlen-Fm0))/(Fmlen-1); 

					db= ((2+2/_afFastFiber)*(-1))/(Fmlen-1); 

				}



				//Compute the last dlce value that falls in the region where

				//Thelen 2003 Eqn. 6 is valid

				double dlce0 = (0.25 + 0.75*a)*(Fm0-afl)/b;



				//Compute the dlce/dfm of Eqn. 6 of Thelen 2003 at the last

				//valid point

				dlcedFm = (0.25 + 0.75*a)*(1)/b - ((0.25 + 0.75*a)*(Fm0-afl)/(b*b))*db;



				//Linearily extrapolate Eqn. 6 from Thelen 2003 to compute

				//the new value for dlce/dFm

				dlce = dlce0 + dlcedFm*(Fm-Fm0);            

			}

	

		return dlce;

}


//_____________________________________________________________________________

/**

 * M.Millard 2011/10 Note: I haven't touched this yet. It appears to work, but 

 * a standard numerical method that is well documented should be used. This is

 * a difficult to read, has unknown convergence properties, and may have bugs.

 *__________________________________________________________________________

 * Find the force produced by an actuator (the musculotendon unit), assuming

 * static equilibrium. Using the total muscle-tendon length, it finds the

 * fiber and tendon lengths so that the forces in each match. This routine

 * takes pennation angle into account, so its definition of static equilibrium

 * is when tendon_force = fiber_force * cos(pennation_angle). This funcion

 * will modify the object's values for length, fiberLength, activeForce, 

 * and passiveForce.

 *

 * @param aActivation Activation of the muscle.

 * @return The isometric force in the muscle.

 */

double DiffMuscle::computeIsometricForce(SimTK::State& s, double aActivation) const
{

#define MAX_ITERATIONS 100

#define ERROR_LIMIT 0.01



   int i;

   double length,tendon_length, fiber_force, tmp_fiber_length, min_tendon_stiffness;

   double cos_factor, fiber_stiffness;

   double old_fiber_length, length_change, tendon_stiffness, percent;

   double error_force = 0.0, old_error_force, tendon_force, norm_tendon_length;

   double passiveForce;

   double fiberLength;



   if (_optimalFiberLength < ROUNDOFF_ERROR) {

      setStateVariable(s, STATE_FIBER_LENGTH, 0.0);

		setPassiveForce(s, 0.0);

      setForce(s, 0.0);

      setTendonForce(s, 0.0);

      return 0.0;

   }



	length = getLength(s);



   // Make first guess of fiber and tendon lengths. Make fiber length equal to

   // optimal_fiber_length so that you start in the middle of the active+passive

   // force-length curve. Muscle_width is the width, or thickness, of the

   // muscle-tendon unit. It is the shortest allowable fiber length because if

   // the muscle-tendon length is very short, the pennation angle will be 90

   // degrees and the fibers will be vertical (assuming the tendon is horizontal).

   // When this happens, the fibers are as long as the muscle is wide.

   // If the resting tendon length is zero, then set the fiber length equal to

   // the muscle tendon length / cosine_factor, and find its force directly.



   double muscle_width = _optimalFiberLength * sin(_pennationAngleAtOptimal);



	if (_tendonSlackLength < ROUNDOFF_ERROR) {

		tendon_length = 0.0;

		cos_factor = cos(atan(muscle_width / length));

		fiberLength = length / cos_factor;



		double activeForce = calcActiveForceFastFiber(s, fiberLength / _optimalFiberLength)  * aActivation;

		if (activeForce < 0.0) activeForce = 0.0;



		passiveForce = calcPassiveForce(s, fiberLength / _optimalFiberLength);

		if (passiveForce < 0.0) passiveForce = 0.0;



		setPassiveForce(s, passiveForce );

		setStateVariable(s, STATE_FIBER_LENGTH, fiberLength);

		tendon_force = (activeForce + passiveForce) * _maxIsometricForce * cos_factor;

		setForce(s, tendon_force);

		setTendonForce(s, tendon_force);

		return tendon_force;

   } else if (length < _tendonSlackLength) {

		setPassiveForce(s, 0.0);

      setStateVariable(s, STATE_FIBER_LENGTH, muscle_width);

      _model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);

      setForce(s, 0.0);

      setTendonForce(s, 0.0);

      return 0.0;

   } else {

      fiberLength = _optimalFiberLength;

      cos_factor = cos(calcPennation( fiberLength, _optimalFiberLength,  _pennationAngleAtOptimal ));  

      tendon_length = length - fiberLength * cos_factor;



      /* Check to make sure tendon is not shorter than its slack length. If it

       * is, set the length to its slack length and re-compute fiber length.

       */



      if (tendon_length < _tendonSlackLength) {

         tendon_length = _tendonSlackLength;

         cos_factor = cos(atan(muscle_width / (length - tendon_length)));

         fiberLength = (length - tendon_length) / cos_factor;

         if (fiberLength < muscle_width)

           fiberLength = muscle_width;

      }



   }



   // Muscle-tendon force is found using an iterative method. First, you guess

   // the length of the muscle fibers and the length of the tendon, and

   // calculate their respective forces. If the forces match (are within

   // ERROR_LIMIT of each other), stop; else change the length guesses based

   // on the error and try again.

   for (i = 0; i < MAX_ITERATIONS; i++) {

		double activeForce = calcActiveForceFastFiber(s, fiberLength/ _optimalFiberLength) * aActivation;

      if( activeForce <  0.0) activeForce = 0.0;



      passiveForce =  calcPassiveForce(s, fiberLength / _optimalFiberLength);

      if (passiveForce < 0.0) passiveForce = 0.0;



      fiber_force = (activeForce + passiveForce) * _maxIsometricForce * cos_factor;



      norm_tendon_length = tendon_length / _optimalFiberLength;

      tendon_force = calcTendonForce(s, norm_tendon_length) * _maxIsometricForce;

		setForce(s, tendon_force);

		setTendonForce(s, tendon_force);



      old_error_force = error_force;

 

      error_force = tendon_force - fiber_force;



      if (DABS(error_force) <= ERROR_LIMIT) // muscle-tendon force found!

         break;



      if (i == 0)

         old_error_force = error_force;



      if (DSIGN(error_force) != DSIGN(old_error_force)) {

         percent = DABS(error_force) / (DABS(error_force) + DABS(old_error_force));

         tmp_fiber_length = old_fiber_length;

         old_fiber_length = fiberLength;

         fiberLength = fiberLength + percent * (tmp_fiber_length - fiberLength);

      } else {

         // Estimate the stiffnesses of the tendon and the fibers. If tendon

         // stiffness is too low, then the next length guess will overshoot

         // the equilibrium point. So we artificially raise it using the

         // normalized muscle force. (_activeForce+_passiveForce) is the

         // normalized force for the current fiber length, and we assume that

         // the equilibrium length is close to this current length. So we want

         // to get force = (_activeForce+_passiveForce) from the tendon as well.

         // We hope this will happen by setting the tendon stiffness to

         // (_activeForce+_passiveForce) times its maximum stiffness.

			double tendon_elastic_modulus = 1200.0;

			double tendon_max_stress = 32.0;



         tendon_stiffness = calcTendonForce(s, norm_tendon_length) *

				_maxIsometricForce / _tendonSlackLength;



         min_tendon_stiffness = (activeForce + passiveForce) *

	         tendon_elastic_modulus * _maxIsometricForce /

	         (tendon_max_stress * _tendonSlackLength);



         if (tendon_stiffness < min_tendon_stiffness)

            tendon_stiffness = min_tendon_stiffness;



         fiber_stiffness = _maxIsometricForce / _optimalFiberLength *

            (calcActiveForceFastFiber(s, fiberLength / _optimalFiberLength)  +

            calcPassiveForce(s, fiberLength / _optimalFiberLength));



         // determine how much the fiber and tendon lengths have to

         // change to make the error_force zero. But don't let the

	      // length change exceed half the optimal fiber length because

	      // that's too big a change to make all at once.

         length_change = fabs(error_force/(fiber_stiffness / cos_factor + tendon_stiffness));



         if (fabs(length_change / _optimalFiberLength) > 0.5)

            length_change = 0.5 * _optimalFiberLength;



         // now change the fiber length depending on the sign of the error

         // and the sign of the fiber stiffness (which equals the sign of

         // the slope of the muscle's force-length curve).

         old_fiber_length = fiberLength;



         if (error_force > 0.0)

             fiberLength += length_change;

         else

             fiberLength -= length_change;





      }

      cos_factor = cos(calcPennation(fiberLength, _optimalFiberLength, _pennationAngleAtOptimal ));

      tendon_length = length - fiberLength * cos_factor;



      // Check to make sure tendon is not shorter than its slack length. If it is,

      // set the length to its slack length and re-compute fiber length.

      if (tendon_length < _tendonSlackLength) {

         tendon_length = _tendonSlackLength;

         cos_factor = cos(atan(muscle_width / (length - tendon_length)));

         fiberLength = (length - tendon_length) / cos_factor ;

      }

	}



	setPassiveForce(s, passiveForce );

	_model->getMultibodySystem().realize(s, SimTK::Stage::Position);

	setStateVariable(s, STATE_FIBER_LENGTH,  fiberLength);



//cout << "ThelenMuscle computeIsometricForce " << getName() << "  t=" << s.getTime() << " force = " << tendon_force << endl;



   return tendon_force;

}



